{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import scipy.linalg as alg\n",
    "import numpy as np\n",
    "import gym\n",
    "from gym import wrappers\n",
    "import gym_custom_envs\n",
    "import time\n",
    "env = gym.make('DoubleIntegrator-v0')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def lqr(A, B, Q, R):\n",
    "    \"\"\"lqr for continous time domain\"\"\"\n",
    "    \n",
    "    # Ricatti equation where S is from cost-to-go J*(x) = x.T*S*x\n",
    "    S = np.matrix(alg.solve_continuous_are(A, B, Q, R))\n",
    "    \n",
    "    # Gain matrix, K from u* = -K*x\n",
    "    K = np.matrix(alg.inv(R)*B.T*S)\n",
    "    \n",
    "    return K, S"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# For double integrator\n",
    "A = np.array([[0., 1.],\n",
    "              [0., 0.]])\n",
    "B = np.array([[0.],\n",
    "              [1.]])\n",
    "Q = np.array([[1., 0.],\n",
    "              [0., 1.]])\n",
    "R = np.array([[1.]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "K =  [[1.         1.73205081]]\n",
      "S =  [[1.73205081 1.        ]\n",
      " [1.         1.73205081]]\n"
     ]
    }
   ],
   "source": [
    "K, S = lqr(A, B, Q, R)\n",
    "print('K = ',K)\n",
    "print('S = ',S)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# from IPython.display import clear_output\n",
    "# env = gym.wrappers.Monitor(env, \"vid\")\n",
    "for i in range(10):\n",
    "    s = env.reset().reshape(1,2)\n",
    "    done = False\n",
    "    while not done:\n",
    "        a = -np.dot(K, s.T.reshape(2,1))\n",
    "#         print(a[0][0])\n",
    "        ns, c, done,_ = env.step(a[0][0])\n",
    "        s = ns\n",
    "        env.render()\n",
    "#         time.sleep(0.005)\n",
    "#         clear_output(wait=True)\n",
    "env.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
